// 2025/4/7: 首个记录
// 2025/6/12: 协议名常量改为public

package com.appox.samples;

import com.appox.Sample;
import com.appox.GeneralSample;
import com.appox.GeneralElement;
import com.appox.Types.FloatPoint;
import java.util.ArrayList;
import java.util.List;

/**
 * 车道线信息样本数据类
 */
public class LaneSensorSample extends Sample {

    /**
     * 线描述方式
     */
    public enum LineDescription {
        MODEL(1), // 通过模型参数和一致的属性进行描述
        SEGMENTS(2), // 通过多段线段和各自的属性进行描述
        BOTH(3); // Model和Segments双模式描述
        
        private final int value;
        
        LineDescription(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static LineDescription fromValue(int value) {
            for (LineDescription desc : LineDescription.values()) {
                if (desc.getValue() == value) {
                    return desc;
                }
            }
            return MODEL;
        }
    }

    /**
     * 线类型
     */
    public enum LineClass {
        GENERAL(1), // 未指定
        DASHED(2), // 虚线
        SOLID(3), // 实线
        DOUBLE_DASHED(4), // 双虚线
        DOUBLE_SOLID(5), // 双实线
        SOLID_DASHED(6), // 左实右虚
        DASHED_SOLID(7), // 左虚右实
        CURB(8), // 路沿
        GUARDRAIL(9), // 护栏
        BARRIER(10); // 障碍物边界
        
        private final int value;
        
        LineClass(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static LineClass fromValue(int value) {
            for (LineClass cls : LineClass.values()) {
                if (cls.getValue() == value) {
                    return cls;
                }
            }
            return GENERAL;
        }
    }

    /**
     * 线颜色
     */
    public enum LineColor {
        UNKNOWN(1), // 未知
        WHITE(2), // 白色
        YELLOW(3), // 黄色
        RED(4), // 红色
        BLUE(5), // 蓝色
        ORANGE(6), // 橙色
        GREEN(7); // 绿色
        
        private final int value;
        
        LineColor(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static LineColor fromValue(int value) {
            for (LineColor color : LineColor.values()) {
                if (color.getValue() == value) {
                    return color;
                }
            }
            return UNKNOWN;
        }
    }

    /**
     * 线段实体属性类
     */
    public static class LineSegmentSolidProperty {
        private LineColor color; // 线段颜色
        private Double width; // [m] 线段宽度
        
        public LineColor getColor() {
            return color;
        }
        
        public void setColor(LineColor color) {
            this.color = color;
        }
        
        public Double getWidth() {
            return width;
        }
        
        public void setWidth(Double width) {
            this.width = width;
        }
    }

    /**
     * 线段类
     */
    public static class LineSegment {
        private int id; // 线段ID
        private FloatPoint p1; // [m] 线段首端点的x-y坐标
        private Float z1; // [m] 线段首端点的z坐标
        private FloatPoint p2; // [m] 线段末端点的x-y坐标
        private Float z2; // [m] 线段末端点的z坐标
        private LineSegmentSolidProperty solidProperty; // 实线段的属性（虚线段则为空）
        
        public int getId() {
            return id;
        }
        
        public void setId(int id) {
            this.id = id;
        }
        
        public FloatPoint getP1() {
            return p1;
        }
        
        public void setP1(FloatPoint p1) {
            this.p1 = p1;
        }
        
        public Float getZ1() {
            return z1;
        }
        
        public void setZ1(Float z1) {
            this.z1 = z1;
        }
        
        public FloatPoint getP2() {
            return p2;
        }
        
        public void setP2(FloatPoint p2) {
            this.p2 = p2;
        }
        
        public Float getZ2() {
            return z2;
        }
        
        public void setZ2(Float z2) {
            this.z2 = z2;
        }
        
        public LineSegmentSolidProperty getSolidProperty() {
            return solidProperty;
        }
        
        public void setSolidProperty(LineSegmentSolidProperty solidProperty) {
            this.solidProperty = solidProperty;
        }
    }

    /**
     * 车道线类
     */
    public static class LaneLine {
        private int id; // 车道线ID，0表示无效
        private Integer rawId; // 车道线原始ID
        private Double confidence; // [%] 车道线置信度
        private LineDescription description; // 车道线的描述方式
        
        private LineClass lineClass; // Model模式：一致性车道线类型
        private LineColor color; // Model模式：一致性车道线颜色
        private Double width; // [m] Model模式：一致性车道线宽度
        private double[] modelCoefs; // Model模式：y = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3
        private double[] modelCoefsZ; // Model模式：z = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3 (空则表示无效)
        private Double rearEnd; // [m] Model模式：曲线后端点位置(x轴)
        private Double frontEnd; // [m] Model模式：曲线前端点位置(x轴)
        
        private List<LineSegment> segments; // Segments模式：线段列表
        
        public LaneLine() {
            id = -1;
            description = LineDescription.MODEL;
            lineClass = LineClass.GENERAL;
            color = LineColor.UNKNOWN;
            modelCoefs = new double[] { 0, 0, 0, 0 };
            segments = new ArrayList<>();
        }
        
        public boolean isModelValid() {
            return description == LineDescription.MODEL || description == LineDescription.BOTH;
        }
        
        public boolean isSegmentsValid() {
            return description == LineDescription.SEGMENTS || description == LineDescription.BOTH;
        }
        
        public int getId() {
            return id;
        }
        
        public void setId(int id) {
            this.id = id;
        }
        
        public Integer getRawId() {
            return rawId;
        }
        
        public void setRawId(Integer rawId) {
            this.rawId = rawId;
        }
        
        public Double getConfidence() {
            return confidence;
        }
        
        public void setConfidence(Double confidence) {
            this.confidence = confidence;
        }
        
        public LineDescription getDescription() {
            return description;
        }
        
        public void setDescription(LineDescription description) {
            this.description = description;
        }
        
        public LineClass getLineClass() {
            return lineClass;
        }
        
        public void setLineClass(LineClass lineClass) {
            this.lineClass = lineClass;
        }
        
        public LineColor getColor() {
            return color;
        }
        
        public void setColor(LineColor color) {
            this.color = color;
        }
        
        public Double getWidth() {
            return width;
        }
        
        public void setWidth(Double width) {
            this.width = width;
        }
        
        public double[] getModelCoefs() {
            return modelCoefs;
        }
        
        public void setModelCoefs(double[] modelCoefs) {
            this.modelCoefs = modelCoefs;
        }
        
        public double[] getModelCoefsZ() {
            return modelCoefsZ;
        }
        
        public void setModelCoefsZ(double[] modelCoefsZ) {
            this.modelCoefsZ = modelCoefsZ;
        }
        
        public Double getRearEnd() {
            return rearEnd;
        }
        
        public void setRearEnd(Double rearEnd) {
            this.rearEnd = rearEnd;
        }
        
        public Double getFrontEnd() {
            return frontEnd;
        }
        
        public void setFrontEnd(Double frontEnd) {
            this.frontEnd = frontEnd;
        }
        
        public List<LineSegment> getSegments() {
            return segments;
        }
        
        public void setSegments(List<LineSegment> segments) {
            this.segments = segments;
        }
    }

    // 车道线
    private List<LaneLine> lines; // 车道线列表
    private Integer firstLeftLineIndex; // 左侧第一条车道线序号
    private Integer firstRightLineIndex; // 右侧第一条车道线序号
    private Integer secondLeftLineIndex; // 左侧第二条车道线序号
    private Integer secondRightLineIndex; // 右侧第二条车道线序号

    // 本车道行驶状态
    private Double lateralVelocity; // [m/s] 当前车道横向速度
    private Double centerDeparture; // [m] 当前车道中央偏离距离

    // 道路属性部分
    private Double laneWidth; // [m] 当前车道宽度
    private Double laneHeading; // [deg] 当前车道朝向
    private Double laneCurvature; // [1/m] 当前车道曲率

    // 车辆参数部分
    private Double vehicleSpeed; // [kph] 本车车速
    private Double vehicleCurvature; // [1/m] 本车行驶曲率
    private Double vehicleAX; // [m/s²] 本车纵向加速度
    private Double vehicleWidth; // [m] 本车宽度
    private Double vehicleFrontOverhang; // [m] 本车前悬
    private Double vehicleWheelBase; // [m] 本车轴距

    // 车道线识别边界
    private double linesRearBound; // [m] 传感器检测范围后边界
    private double linesFrontBound; // [m] 传感器检测范围前边界

    /**
     * 构造函数
     */
    public LaneSensorSample() {
        lines = new ArrayList<>();
        linesRearBound = 0;
        linesFrontBound = 30;
    }

    /**
     * 获取车道线列表
     */
    public List<LaneLine> getLines() {
        return lines;
    }
    
    /**
     * 设置车道线列表
     */
    public void setLines(List<LaneLine> lines) {
        this.lines = lines;
    }
    
    /**
     * 获取左侧第一条车道线序号
     */
    public Integer getFirstLeftLineIndex() {
        return firstLeftLineIndex;
    }
    
    /**
     * 设置左侧第一条车道线序号
     */
    public void setFirstLeftLineIndex(Integer firstLeftLineIndex) {
        this.firstLeftLineIndex = firstLeftLineIndex;
    }
    
    /**
     * 获取右侧第一条车道线序号
     */
    public Integer getFirstRightLineIndex() {
        return firstRightLineIndex;
    }
    
    /**
     * 设置右侧第一条车道线序号
     */
    public void setFirstRightLineIndex(Integer firstRightLineIndex) {
        this.firstRightLineIndex = firstRightLineIndex;
    }
    
    /**
     * 获取左侧第二条车道线序号
     */
    public Integer getSecondLeftLineIndex() {
        return secondLeftLineIndex;
    }
    
    /**
     * 设置左侧第二条车道线序号
     */
    public void setSecondLeftLineIndex(Integer secondLeftLineIndex) {
        this.secondLeftLineIndex = secondLeftLineIndex;
    }
    
    /**
     * 获取右侧第二条车道线序号
     */
    public Integer getSecondRightLineIndex() {
        return secondRightLineIndex;
    }
    
    /**
     * 设置右侧第二条车道线序号
     */
    public void setSecondRightLineIndex(Integer secondRightLineIndex) {
        this.secondRightLineIndex = secondRightLineIndex;
    }
    
    /**
     * 获取当前车道横向速度
     */
    public Double getLateralVelocity() {
        return lateralVelocity;
    }
    
    /**
     * 设置当前车道横向速度
     */
    public void setLateralVelocity(Double lateralVelocity) {
        this.lateralVelocity = lateralVelocity;
    }
    
    /**
     * 获取当前车道中央偏离距离
     */
    public Double getCenterDeparture() {
        return centerDeparture;
    }
    
    /**
     * 设置当前车道中央偏离距离
     */
    public void setCenterDeparture(Double centerDeparture) {
        this.centerDeparture = centerDeparture;
    }
    
    /**
     * 获取当前车道宽度
     */
    public Double getLaneWidth() {
        return laneWidth;
    }
    
    /**
     * 设置当前车道宽度
     */
    public void setLaneWidth(Double laneWidth) {
        this.laneWidth = laneWidth;
    }
    
    /**
     * 获取当前车道朝向
     */
    public Double getLaneHeading() {
        return laneHeading;
    }
    
    /**
     * 设置当前车道朝向
     */
    public void setLaneHeading(Double laneHeading) {
        this.laneHeading = laneHeading;
    }
    
    /**
     * 获取当前车道曲率
     */
    public Double getLaneCurvature() {
        return laneCurvature;
    }
    
    /**
     * 设置当前车道曲率
     */
    public void setLaneCurvature(Double laneCurvature) {
        this.laneCurvature = laneCurvature;
    }
    
    /**
     * 获取本车车速
     */
    public Double getVehicleSpeed() {
        return vehicleSpeed;
    }
    
    /**
     * 设置本车车速
     */
    public void setVehicleSpeed(Double vehicleSpeed) {
        this.vehicleSpeed = vehicleSpeed;
    }
    
    /**
     * 获取本车行驶曲率
     */
    public Double getVehicleCurvature() {
        return vehicleCurvature;
    }
    
    /**
     * 设置本车行驶曲率
     */
    public void setVehicleCurvature(Double vehicleCurvature) {
        this.vehicleCurvature = vehicleCurvature;
    }
    
    /**
     * 获取本车纵向加速度
     */
    public Double getVehicleAX() {
        return vehicleAX;
    }
    
    /**
     * 设置本车纵向加速度
     */
    public void setVehicleAX(Double vehicleAX) {
        this.vehicleAX = vehicleAX;
    }
    
    /**
     * 获取本车宽度
     */
    public Double getVehicleWidth() {
        return vehicleWidth;
    }
    
    /**
     * 设置本车宽度
     */
    public void setVehicleWidth(Double vehicleWidth) {
        this.vehicleWidth = vehicleWidth;
    }
    
    /**
     * 获取本车前悬
     */
    public Double getVehicleFrontOverhang() {
        return vehicleFrontOverhang;
    }
    
    /**
     * 设置本车前悬
     */
    public void setVehicleFrontOverhang(Double vehicleFrontOverhang) {
        this.vehicleFrontOverhang = vehicleFrontOverhang;
    }
    
    /**
     * 获取本车轴距
     */
    public Double getVehicleWheelBase() {
        return vehicleWheelBase;
    }
    
    /**
     * 设置本车轴距
     */
    public void setVehicleWheelBase(Double vehicleWheelBase) {
        this.vehicleWheelBase = vehicleWheelBase;
    }
    
    /**
     * 获取传感器检测范围后边界
     */
    public double getLinesRearBound() {
        return linesRearBound;
    }
    
    /**
     * 设置传感器检测范围后边界
     */
    public void setLinesRearBound(double linesRearBound) {
        this.linesRearBound = linesRearBound;
    }
    
    /**
     * 获取传感器检测范围前边界
     */
    public double getLinesFrontBound() {
        return linesFrontBound;
    }
    
    /**
     * 设置传感器检测范围前边界
     */
    public void setLinesFrontBound(double linesFrontBound) {
        this.linesFrontBound = linesFrontBound;
    }
    
    /**
     * 获取通用样本协议
     */
    @Override
    public String getGeneralSampleProtocol() {
        return TARGET_PROTOCOL;
    }
    
    /**
     * 获取通用样本协议列表
     */
    @Override
    public String[] getGeneralSampleProtocols() {
        return PROTOCOLS;
    }
    
    /**
     * 将车道线传感器样本转换为通用样本
     */
    public GeneralSample toGeneralSample(int channel) {
        GeneralSample sample = new GeneralSample();
        sample.setTime(this);
        sample.setProtocol(TARGET_PROTOCOL);
        sample.setChannel(channel);
        
        int segmentCount = 0;
        int[] segmentOffset = new int[lines.size()];
        int[] segmentSize = new int[lines.size()];
        for (int i = 0; i < lines.size(); i++) {
            segmentOffset[i] = segmentCount;
            segmentSize[i] = lines.get(i).getSegments().size();
            segmentCount += segmentSize[i];
        }
        
        GeneralElement[] data = new GeneralElement[23 + lines.size() * 27 + segmentCount * 12];
        
        // 初始化所有元素
        for (int i = 0; i < data.length; i++) {
            data[i] = new GeneralElement();
        }
        
        data[0] = new GeneralElement((double)lines.size());
        data[1] = new GeneralElement((double)segmentCount);
        
        if (lateralVelocity != null) data[2] = new GeneralElement(lateralVelocity);
        if (centerDeparture != null) data[3] = new GeneralElement(centerDeparture);
        if (laneWidth != null) data[4] = new GeneralElement(laneWidth);
        if (laneHeading != null) data[5] = new GeneralElement(laneHeading);
        if (laneCurvature != null) data[6] = new GeneralElement(laneCurvature);
        data[7] = new GeneralElement(linesRearBound);
        data[8] = new GeneralElement(linesFrontBound);
        if (firstLeftLineIndex != null) data[9] = new GeneralElement((double)firstLeftLineIndex);
        if (firstRightLineIndex != null) data[10] = new GeneralElement((double)firstRightLineIndex);
        if (secondLeftLineIndex != null) data[11] = new GeneralElement((double)secondLeftLineIndex);
        if (secondRightLineIndex != null) data[12] = new GeneralElement((double)secondRightLineIndex);
        if (vehicleSpeed != null) data[13] = new GeneralElement(vehicleSpeed);
        if (vehicleCurvature != null) data[14] = new GeneralElement(vehicleCurvature);
        if (vehicleAX != null) data[15] = new GeneralElement(vehicleAX);
        if (vehicleWidth != null) data[16] = new GeneralElement(vehicleWidth);
        if (vehicleFrontOverhang != null) data[17] = new GeneralElement(vehicleFrontOverhang);
        if (vehicleWheelBase != null) data[18] = new GeneralElement(vehicleWheelBase);
        
        int segBase = 23 + lines.size() * 27;
        
        for (int i = 0; i < lines.size(); i++) {
            LaneLine src = lines.get(i);
            int baseIndex = 23 + 27 * i;
            
            data[baseIndex + 0] = new GeneralElement((double)src.getId());
            if (src.getRawId() != null) data[baseIndex + 1] = new GeneralElement((double)src.getRawId());
            if (src.getConfidence() != null) data[baseIndex + 2] = new GeneralElement(src.getConfidence());
            data[baseIndex + 3] = new GeneralElement((double)src.getDescription().getValue());
            
            if (src.isModelValid()) {
                data[baseIndex + 4] = new GeneralElement((double)src.getLineClass().getValue());
                data[baseIndex + 5] = new GeneralElement((double)src.getColor().getValue());
                if (src.getWidth() != null) data[baseIndex + 6] = new GeneralElement(src.getWidth());
                data[baseIndex + 7] = new GeneralElement(src.getModelCoefs()[0]);
                data[baseIndex + 8] = new GeneralElement(src.getModelCoefs()[1]);
                data[baseIndex + 9] = new GeneralElement(src.getModelCoefs()[2]);
                data[baseIndex + 10] = new GeneralElement(src.getModelCoefs()[3]);
                if (src.getModelCoefsZ() == null) data[baseIndex + 11] = new GeneralElement(0.0);
                else {
                    data[baseIndex + 11] = new GeneralElement(1.0);
                    data[baseIndex + 12] = new GeneralElement(src.getModelCoefsZ()[0]);
                    data[baseIndex + 13] = new GeneralElement(src.getModelCoefsZ()[1]);
                    data[baseIndex + 14] = new GeneralElement(src.getModelCoefsZ()[2]);
                    data[baseIndex + 15] = new GeneralElement(src.getModelCoefsZ()[3]);
                }
                if (src.getRearEnd() != null) data[baseIndex + 16] = new GeneralElement(src.getRearEnd());
                if (src.getFrontEnd() != null) data[baseIndex + 17] = new GeneralElement(src.getFrontEnd());
            }
            
            if (src.isSegmentsValid()) {
                int segOffset = segmentOffset[i];
                int segSize = segmentSize[i];
                data[baseIndex + 25] = new GeneralElement((double)segOffset);
                data[baseIndex + 26] = new GeneralElement((double)segSize);
                if (segSize > 0) {
                    for (int n = 0; n < segSize; n++) {
                        LineSegment seg = src.getSegments().get(n);
                        int segBaseIndex = segBase + 12 * (segOffset + n);
                        
                        data[segBaseIndex + 0] = new GeneralElement((double)seg.getId());
                        data[segBaseIndex + 1] = new GeneralElement((double)seg.getP1().getX());
                        data[segBaseIndex + 2] = new GeneralElement((double)seg.getP1().getY());
                        if (seg.getZ1() != null) data[segBaseIndex + 3] = new GeneralElement((double)seg.getZ1());
                        data[segBaseIndex + 4] = new GeneralElement((double)seg.getP2().getX());
                        data[segBaseIndex + 5] = new GeneralElement((double)seg.getP2().getY());
                        if (seg.getZ2() != null) data[segBaseIndex + 6] = new GeneralElement((double)seg.getZ2());
                        if (seg.getSolidProperty() == null) data[segBaseIndex + 7] = new GeneralElement(0.0);
                        else {
                            data[segBaseIndex + 7] = new GeneralElement(1.0);
                            data[segBaseIndex + 8] = new GeneralElement((double)seg.getSolidProperty().getColor().getValue());
                            if (seg.getSolidProperty().getWidth() != null) data[segBaseIndex + 9] = new GeneralElement(seg.getSolidProperty().getWidth());
                        }
                    }
                }
            }
            
            Double dtlc = getDTLC(src);
            if (dtlc != null) data[baseIndex + 21] = new GeneralElement(dtlc);
            Double stlc = getSTLC(src);
            if (stlc != null) data[baseIndex + 22] = new GeneralElement(stlc);
            Double ttlc = getTTLC(src);
            if (ttlc != null) data[baseIndex + 23] = new GeneralElement(ttlc);
        }
        
        sample.setValues(data);
        sample.setNumberOfSignificants(data.length);
        
        return sample;
    }
    
    /**
     * 从通用样本转换为车道线传感器样本
     */
    @Override
    public boolean fromGeneralSample(GeneralSample sample) {
        setTime(sample);
        
        String protocol = sample.getProtocol();
        if ("lane-sensor-sample-v4".equals(protocol)) {
            return fromLaneSensorSampleV4(sample);
        } else if ("lane-sensor-sample-v3".equals(protocol)) {
            return fromLaneSensorSampleV3(sample);
        } else if ("lane-sensor-sample-v2".equals(protocol)) {
            return fromLaneSensorSampleV2(sample);
        }
        
        return false;
    }
    
    /**
     * 从V4版本通用样本转换为车道线传感器样本
     */
    private boolean fromLaneSensorSampleV4(GeneralSample sample) {
        GeneralElement[] v = sample.getValues();
        if (v == null || v.length < 23) return false;
        if (!v[0].isValid() || !v[1].isValid()) return false;
        
        int lineCount = (int)v[0].getValue();
        int segmentCount = (int)v[1].getValue();
        if (v.length != 23 + lineCount * 27 + segmentCount * 12) return false;
        
        lateralVelocity = v[2].isValid() ? v[2].getValue() : null;
        centerDeparture = v[3].isValid() ? v[3].getValue() : null;
        laneWidth = v[4].isValid() ? v[4].getValue() : null;
        laneHeading = v[5].isValid() ? v[5].getValue() : null;
        laneCurvature = v[6].isValid() ? v[6].getValue() : null;
        linesRearBound = v[7].getValue();
        linesFrontBound = v[8].getValue();
        firstLeftLineIndex = v[9].isValid() ? (int)v[9].getValue() : null;
        firstRightLineIndex = v[10].isValid() ? (int)v[10].getValue() : null;
        secondLeftLineIndex = v[11].isValid() ? (int)v[11].getValue() : null;
        secondRightLineIndex = v[12].isValid() ? (int)v[12].getValue() : null;
        vehicleSpeed = v[13].isValid() ? v[13].getValue() : null;
        vehicleCurvature = v[14].isValid() ? v[14].getValue() : null;
        vehicleAX = v[15].isValid() ? v[15].getValue() : null;
        vehicleWidth = v[16].isValid() ? v[16].getValue() : null;
        vehicleFrontOverhang = v[17].isValid() ? v[17].getValue() : null;
        vehicleWheelBase = v[18].isValid() ? v[18].getValue() : null;
        
        int segBase = 23 + lineCount * 27;
        
        lines.clear();
        for (int i = 0; i < lineCount; i++) {
            LaneLine line = new LaneLine();
            int baseIndex = 23 + 27 * i;
            
            line.setId((int)v[baseIndex + 0].getValue());
            line.setRawId(v[baseIndex + 1].isValid() ? (int)v[baseIndex + 1].getValue() : null);
            line.setConfidence(v[baseIndex + 2].isValid() ? v[baseIndex + 2].getValue() : null);
            line.setDescription(LineDescription.fromValue((int)v[baseIndex + 3].getValue()));
            
            if (line.isModelValid()) {
                line.setLineClass(LineClass.fromValue((int)v[baseIndex + 4].getValue()));
                line.setColor(LineColor.fromValue((int)v[baseIndex + 5].getValue()));
                line.setWidth(v[baseIndex + 6].isValid() ? v[baseIndex + 6].getValue() : null);
                line.getModelCoefs()[0] = v[baseIndex + 7].getValue();
                line.getModelCoefs()[1] = v[baseIndex + 8].getValue();
                line.getModelCoefs()[2] = v[baseIndex + 9].getValue();
                line.getModelCoefs()[3] = v[baseIndex + 10].getValue();
                if (v[baseIndex + 11].getValue() == 1) {
                    line.setModelCoefsZ(new double[4]);
                    line.getModelCoefsZ()[0] = v[baseIndex + 12].getValue();
                    line.getModelCoefsZ()[1] = v[baseIndex + 13].getValue();
                    line.getModelCoefsZ()[2] = v[baseIndex + 14].getValue();
                    line.getModelCoefsZ()[3] = v[baseIndex + 15].getValue();
                }
                line.setRearEnd(v[baseIndex + 16].isValid() ? v[baseIndex + 16].getValue() : null);
                line.setFrontEnd(v[baseIndex + 17].isValid() ? v[baseIndex + 17].getValue() : null);
            }
            
            if (line.isSegmentsValid()) {
                int segOffset = (int)v[baseIndex + 25].getValue();
                int segSize = (int)v[baseIndex + 26].getValue();
                if (segSize > 0) {
                    for (int n = 0; n < segSize; n++) {
                        LineSegment seg = new LineSegment();
                        int segBaseIndex = segBase + 12 * (segOffset + n);
                        
                        seg.setId((int)v[segBaseIndex + 0].getValue());
                        
                        FloatPoint p1 = new FloatPoint();
                        p1.setX((float)v[segBaseIndex + 1].getValue());
                        p1.setY((float)v[segBaseIndex + 2].getValue());
                        seg.setP1(p1);
                        
                        seg.setZ1(v[segBaseIndex + 3].isValid() ? (float)v[segBaseIndex + 3].getValue() : null);
                        
                        FloatPoint p2 = new FloatPoint();
                        p2.setX((float)v[segBaseIndex + 4].getValue());
                        p2.setY((float)v[segBaseIndex + 5].getValue());
                        seg.setP2(p2);
                        
                        seg.setZ2(v[segBaseIndex + 6].isValid() ? (float)v[segBaseIndex + 6].getValue() : null);
                        
                        if (v[segBaseIndex + 7].getValue() == 1) {
                            LineSegmentSolidProperty prop = new LineSegmentSolidProperty();
                            prop.setColor(LineColor.fromValue((int)v[segBaseIndex + 8].getValue()));
                            prop.setWidth(v[segBaseIndex + 9].isValid() ? v[segBaseIndex + 9].getValue() : null);
                            seg.setSolidProperty(prop);
                        }
                        
                        line.getSegments().add(seg);
                    }
                }
            }
            
            lines.add(line);
        }
        
        return true;
    }
    
    /**
     * 从V3版本通用样本转换为车道线传感器样本
     */
    private boolean fromLaneSensorSampleV3(GeneralSample sample) {
        GeneralElement[] v = sample.getValues();
        if (v == null || v.length < 17) return false;
        if (!v[0].isValid()) return false;
        
        int lineCount = (int)v[0].getValue();
        if (v.length != 17 + lineCount * 15) return false;
        
        lateralVelocity = v[1].isValid() ? v[1].getValue() : null;
        centerDeparture = v[2].isValid() ? v[2].getValue() : null;
        laneWidth = v[3].isValid() ? v[3].getValue() : null;
        laneHeading = v[4].isValid() ? v[4].getValue() : null;
        laneCurvature = v[5].isValid() ? v[5].getValue() : null;
        vehicleSpeed = v[6].isValid() ? v[6].getValue() : null;
        vehicleCurvature = v[7].isValid() ? v[7].getValue() : null;
        vehicleAX = v[8].isValid() ? v[8].getValue() : null;
        vehicleWidth = v[9].isValid() ? v[9].getValue() : null;
        vehicleFrontOverhang = v[10].isValid() ? v[10].getValue() : null;
        linesRearBound = v[11].getValue();
        linesFrontBound = v[12].getValue();
        firstLeftLineIndex = v[13].isValid() ? (int)v[13].getValue() : null;
        firstRightLineIndex = v[14].isValid() ? (int)v[14].getValue() : null;
        secondLeftLineIndex = v[15].isValid() ? (int)v[15].getValue() : null;
        secondRightLineIndex = v[16].isValid() ? (int)v[16].getValue() : null;
        
        lines.clear();
        for (int i = 0; i < lineCount; i++) {
            LaneLine line = new LaneLine();
            int baseIndex = 17 + 15 * i;
            
            line.setId((int)v[baseIndex + 0].getValue());
            line.setRawId(v[baseIndex + 1].isValid() ? (int)v[baseIndex + 1].getValue() : null);
            line.setLineClass(LineClass.fromValue((int)v[baseIndex + 2].getValue()));
            line.setColor(LineColor.fromValue((int)v[baseIndex + 3].getValue()));
            line.setConfidence(v[baseIndex + 4].isValid() ? v[baseIndex + 4].getValue() : null);
            line.setWidth(v[baseIndex + 5].isValid() ? v[baseIndex + 5].getValue() : null);
            line.getModelCoefs()[0] = v[baseIndex + 6].getValue();
            line.getModelCoefs()[1] = v[baseIndex + 7].getValue();
            line.getModelCoefs()[2] = v[baseIndex + 8].getValue();
            line.getModelCoefs()[3] = v[baseIndex + 9].getValue();
            line.setRearEnd(v[baseIndex + 10].isValid() ? v[baseIndex + 10].getValue() : null);
            line.setFrontEnd(v[baseIndex + 11].isValid() ? v[baseIndex + 11].getValue() : null);
            
            lines.add(line);
        }
        
        return true;
    }
    
    /**
     * 从V2版本通用样本转换为车道线传感器样本
     */
    private boolean fromLaneSensorSampleV2(GeneralSample sample) {
        GeneralElement[] v = sample.getValues();
        if (v == null || v.length < 30) return false;
        
        boolean leftDetected = v[0].getValue() == 1;
        boolean rightDetected = v[1].getValue() == 1;
        
        lines.clear();
        
        int index = 0;
        if (leftDetected) {
            LaneLine newLine = new LaneLine();
            
            newLine.setModelCoefs(new double[] {
                v[2].getValue(),
                v[3].getValue(),
                v[4].getValue(),
                v[5].getValue()
            });
            newLine.setLineClass(LineClass.fromValue((int)v[6].getValue()));
            
            lines.add(newLine);
            firstLeftLineIndex = index++;
        }
        if (rightDetected) {
            LaneLine newLine = new LaneLine();
            
            newLine.setModelCoefs(new double[] {
                v[8].getValue(),
                v[9].getValue(),
                v[10].getValue(),
                v[11].getValue()
            });
            newLine.setLineClass(LineClass.fromValue((int)v[12].getValue()));
            
            lines.add(newLine);
            firstRightLineIndex = index++;
        }
        
        laneWidth = v[14].isValid() ? v[14].getValue() : null;
        laneHeading = v[15].isValid() ? v[15].getValue() : null;
        laneCurvature = v[16].isValid() ? v[16].getValue() : null;
        
        lateralVelocity = v[23].isValid() ? v[23].getValue() : null;
        centerDeparture = v[24].isValid() ? v[24].getValue() : null;
        
        vehicleSpeed = v[25].isValid() ? v[25].getValue() : null;
        vehicleCurvature = v[26].isValid() ? v[26].getValue() : null;
        vehicleAX = v[27].isValid() ? v[27].getValue() : null;
        vehicleWidth = v[28].isValid() ? v[28].getValue() : null;
        vehicleFrontOverhang = v[29].isValid() ? v[29].getValue() : null;
        
        return true;
    }
    
    /**
     * 计算车道线的DTLC（Distance To Lane Crossing，到车道线的距离）
     */
    public Double getDTLC(LaneLine line) {
        if (line.isModelValid() && vehicleWidth != null && vehicleFrontOverhang != null) {
            double[] values = line.getModelCoefs();
            boolean isLeft = values[0] > 0;
            
            double x = -1 * vehicleFrontOverhang;
            double y = (values[3] * x * x * x + values[2] * x * x
                + values[1] * x + values[0]) * (isLeft ? 1 : -1);
            double cosTheta = Math.abs(Math.cos(Math.atan(values[1])));
            return cosTheta * (y - vehicleWidth / 2.0);
        } else {
            return null;
        }
    }
    
    /**
     * 计算车道线的STLC（Speed To Lane Crossing，横向速度）
     */
    public Double getSTLC(LaneLine line) {
        if (line.isModelValid() && vehicleSpeed != null) {
            double[] values = line.getModelCoefs();
            boolean isLeft = values[0] > 0;
            
            double curv = vehicleCurvature == null ? 0 : vehicleCurvature;
            double fo = vehicleFrontOverhang == null ? 0.9 : vehicleFrontOverhang;
            double wb = vehicleWheelBase == null ? 2.8 : vehicleWheelBase;
            
            double sinTheta = Math.sin(Math.atan(values[1] - curv * (fo + wb)));
            return (isLeft ? -1.0 : 1.0) * sinTheta * vehicleSpeed / 3.6;
        } else {
            return null;
        }
    }
    
    /**
     * 计算车道线的TTLC（Time To Lane Crossing，到车道线的时间）
     */
    public Double getTTLC(LaneLine line) {
        if (line.isModelValid() && vehicleSpeed != null && vehicleWidth != null && vehicleFrontOverhang != null) {
            if (vehicleSpeed <= 0) return null;
            
            double[] values = line.getModelCoefs();
            boolean isLeft = values[0] > 0;
            
            double wb = vehicleWheelBase != null ? vehicleWheelBase : 2.8;
            double x0 = -vehicleFrontOverhang - wb;
            double x = x0;
            double y = 0;
            double vx = vehicleSpeed / 3.6;
            
            int loopTimes = vx < 5 ? 1000 : (int)(50.0 / vx * 100); // 超过50米越线则不输出（精度无法保证）
            
            double rx = wb;
            double ry = (isLeft ? 0.5 : -0.5) * vehicleWidth;
            
            for (int i = 0; i <= loopTimes; i++) {
                double k = 0;
                if (vehicleCurvature != null) k = vehicleCurvature * (x - x0);
                
                double cosa = 1.0 / Math.sqrt(1 + k * k);
                double sina = cosa * k;
                
                double tx = x + cosa * rx - sina * ry;
                double ty = y + sina * rx + cosa * ry;
                double ly = values[3] * tx * tx * tx + values[2] * tx * tx + values[1] * tx + values[0];
                
                boolean passed = false;
                if (isLeft) {
                    if (ly <= ty) passed = true;
                } else {
                    if (ly >= ty) passed = true;
                }
                if (passed) {
                    if (i == 0) return null;
                    else return (double)i / 100;
                }
                
                if (vehicleAX != null) vx += vehicleAX * 0.01;
                if (vx <= 0) break;
                
                x += vx * 0.01 * cosa;
                y += vx * 0.01 * sina;
            }
            
            return null;
        } else {
            return null;
        }
    }

    /**
     * 是否支持插值
     */
    @Override
    public boolean supportInterpolation() {
        return true;
    }
    
    /**
     * 插值方法，生成两个样本之间的插值样本
     */
    @Override
    public Sample interpolate(SearchResult input) {
        LaneSensorSample s1 = (LaneSensorSample)input.getS1();
        LaneSensorSample s2 = (LaneSensorSample)input.getS2();
        double w1 = input.getW1();
        double w2 = input.getW2();
        
        // 检查车道线ID是否有效
        boolean noID = false;
        for (LaneLine line : s1.getLines()) {
            if (line.getId() <= 0) {
                noID = true;
                break;
            }
        }
        for (LaneLine line : s2.getLines()) {
            if (line.getId() <= 0) {
                noID = true;
                break;
            }
        }
        
        // 如果ID无效，返回权重较大的样本
        if (noID) {
            return w1 > w2 ? s1 : s2;
        }
        
        // 查找公共车道线
        Integer firstLeftID = null, firstRightID = null;
        if (s1.getFirstLeftLineIndex() != null && s2.getFirstLeftLineIndex() != null &&
            s1.getLines().get(s1.getFirstLeftLineIndex()).getId() == s2.getLines().get(s2.getFirstLeftLineIndex()).getId()) {
            firstLeftID = s1.getLines().get(s1.getFirstLeftLineIndex()).getId();
        }
        if (s1.getFirstRightLineIndex() != null && s2.getFirstRightLineIndex() != null &&
            s1.getLines().get(s1.getFirstRightLineIndex()).getId() == s2.getLines().get(s2.getFirstRightLineIndex()).getId()) {
            firstRightID = s1.getLines().get(s1.getFirstRightLineIndex()).getId();
        }
        
        Integer secondLeftID = null, secondRightID = null;
        if (s1.getSecondLeftLineIndex() != null && s2.getSecondLeftLineIndex() != null &&
            s1.getLines().get(s1.getSecondLeftLineIndex()).getId() == s2.getLines().get(s2.getSecondLeftLineIndex()).getId()) {
            secondLeftID = s1.getLines().get(s1.getSecondLeftLineIndex()).getId();
        }
        if (s1.getSecondRightLineIndex() != null && s2.getSecondRightLineIndex() != null &&
            s1.getLines().get(s1.getSecondRightLineIndex()).getId() == s2.getLines().get(s2.getSecondRightLineIndex()).getId()) {
            secondRightID = s1.getLines().get(s1.getSecondRightLineIndex()).getId();
        }
        
        // 创建新样本
        LaneSensorSample output = new LaneSensorSample();
        
        // 插值车道状态数据
        output.setLateralVelocity(interpolateDouble(s1.getLateralVelocity(), s2.getLateralVelocity(), w1, w2));
        output.setCenterDeparture(interpolateDouble(s1.getCenterDeparture(), s2.getCenterDeparture(), w1, w2));
        
        // 插值车道属性数据
        output.setLaneWidth(interpolateDouble(s1.getLaneWidth(), s2.getLaneWidth(), w1, w2));
        output.setLaneCurvature(interpolateDouble(s1.getLaneCurvature(), s2.getLaneCurvature(), w1, w2));
        output.setLaneHeading(interpolateDouble(s1.getLaneHeading(), s2.getLaneHeading(), w1, w2));
        
        // 插值车辆数据
        output.setVehicleSpeed(interpolateDouble(s1.getVehicleSpeed(), s2.getVehicleSpeed(), w1, w2));
        output.setVehicleCurvature(interpolateDouble(s1.getVehicleCurvature(), s2.getVehicleCurvature(), w1, w2));
        output.setVehicleAX(interpolateDouble(s1.getVehicleAX(), s2.getVehicleAX(), w1, w2));
        output.setVehicleWidth(s1.getVehicleWidth());
        output.setVehicleFrontOverhang(s1.getVehicleFrontOverhang());
        output.setVehicleWheelBase(s1.getVehicleWheelBase());
        
        // 保留检测范围
        output.setLinesRearBound(w1 > w2 ? s1.getLinesRearBound() : s2.getLinesRearBound());
        output.setLinesFrontBound(w1 > w2 ? s1.getLinesFrontBound() : s2.getLinesFrontBound());
        
        // 处理车道线
        List<LaneLine> lines = new ArrayList<>();
        int index = 0;
        for (int i = 0; i < s1.getLines().size(); i++) {
            LaneLine s1Line = s1.getLines().get(i);
            for (int j = 0; j < s2.getLines().size(); j++) {
                LaneLine s2Line = s2.getLines().get(j);
                
                if (s1Line.getId() == s2Line.getId() && s1Line.getDescription() == s2Line.getDescription()) {
                    // 检查是否是重要车道线
                    if (firstLeftID != null && firstLeftID == s1Line.getId()) output.setFirstLeftLineIndex(index);
                    if (firstRightID != null && firstRightID == s1Line.getId()) output.setFirstRightLineIndex(index);
                    if (secondLeftID != null && secondLeftID == s1Line.getId()) output.setSecondLeftLineIndex(index);
                    if (secondRightID != null && secondRightID == s1Line.getId()) output.setSecondRightLineIndex(index);
                    
                    // 创建插值车道线
                    LaneLine line = new LaneLine();
                    line.setId(s1Line.getId());
                    line.setRawId(s1Line.getRawId());
                    line.setConfidence(interpolateDouble(s1Line.getConfidence(), s2Line.getConfidence(), w1, w2));
                    line.setDescription(s1Line.getDescription());
                    
                    // 处理Model模式数据
                    if (line.isModelValid()) {
                        line.setLineClass(w1 > w2 ? s1Line.getLineClass() : s2Line.getLineClass());
                        line.setColor(w1 > w2 ? s1Line.getColor() : s2Line.getColor());
                        line.setWidth(interpolateDouble(s1Line.getWidth(), s2Line.getWidth(), w1, w2));
                        
                        // 插值模型系数
                        line.getModelCoefs()[0] = s1Line.getModelCoefs()[0] * w1 + s2Line.getModelCoefs()[0] * w2;
                        line.getModelCoefs()[1] = s1Line.getModelCoefs()[1] * w1 + s2Line.getModelCoefs()[1] * w2;
                        line.getModelCoefs()[2] = s1Line.getModelCoefs()[2] * w1 + s2Line.getModelCoefs()[2] * w2;
                        line.getModelCoefs()[3] = s1Line.getModelCoefs()[3] * w1 + s2Line.getModelCoefs()[3] * w2;
                        
                        // 插值Z轴模型系数
                        if (s1Line.getModelCoefsZ() != null && s2Line.getModelCoefsZ() != null) {
                            line.setModelCoefsZ(new double[4]);
                            line.getModelCoefsZ()[0] = s1Line.getModelCoefsZ()[0] * w1 + s2Line.getModelCoefsZ()[0] * w2;
                            line.getModelCoefsZ()[1] = s1Line.getModelCoefsZ()[1] * w1 + s2Line.getModelCoefsZ()[1] * w2;
                            line.getModelCoefsZ()[2] = s1Line.getModelCoefsZ()[2] * w1 + s2Line.getModelCoefsZ()[2] * w2;
                            line.getModelCoefsZ()[3] = s1Line.getModelCoefsZ()[3] * w1 + s2Line.getModelCoefsZ()[3] * w2;
                        }
                        
                        line.setRearEnd(interpolateDouble(s1Line.getRearEnd(), s2Line.getRearEnd(), w1, w2));
                        line.setFrontEnd(interpolateDouble(s1Line.getFrontEnd(), s2Line.getFrontEnd(), w1, w2));
                    }
                    
                    // 处理Segments模式数据
                    if (line.isSegmentsValid()) {
                        for (int si = 0; si < s1Line.getSegments().size(); si++) {
                            LineSegment s1Seg = s1Line.getSegments().get(si);
                            for (int sj = 0; sj < s2Line.getSegments().size(); sj++) {
                                LineSegment s2Seg = s2Line.getSegments().get(sj);
                                
                                if (s1Seg.getId() == s2Seg.getId()) {
                                    // 创建插值线段
                                    LineSegment seg = new LineSegment();
                                    seg.setId(s1Seg.getId());
                                    
                                    // 插值端点坐标
                                    FloatPoint p1 = new FloatPoint();
                                    p1.setX((float)(s1Seg.getP1().getX() * w1 + s2Seg.getP1().getX() * w2));
                                    p1.setY((float)(s1Seg.getP1().getY() * w1 + s2Seg.getP1().getY() * w2));
                                    seg.setP1(p1);
                                    
                                    // 插值Z坐标
                                    Float z1 = null;
                                    if (s1Seg.getZ1() != null && s2Seg.getZ1() != null) {
                                        z1 = (float)(s1Seg.getZ1() * w1 + s2Seg.getZ1() * w2);
                                    }
                                    seg.setZ1(z1);
                                    
                                    FloatPoint p2 = new FloatPoint();
                                    p2.setX((float)(s1Seg.getP2().getX() * w1 + s2Seg.getP2().getX() * w2));
                                    p2.setY((float)(s1Seg.getP2().getY() * w1 + s2Seg.getP2().getY() * w2));
                                    seg.setP2(p2);
                                    
                                    Float z2 = null;
                                    if (s1Seg.getZ2() != null && s2Seg.getZ2() != null) {
                                        z2 = (float)(s1Seg.getZ2() * w1 + s2Seg.getZ2() * w2);
                                    }
                                    seg.setZ2(z2);
                                    
                                    // 保留权重较大样本的实线属性
                                    seg.setSolidProperty(w1 > w2 ? s1Seg.getSolidProperty() : s2Seg.getSolidProperty());
                                    
                                    line.getSegments().add(seg);
                                }
                            }
                        }
                    }
                    
                    lines.add(line);
                    index++;
                    break;
                }
            }
        }
        output.setLines(lines);
        
        return output;
    }
    
    /**
     * 插值Double数值
     */
    private Double interpolateDouble(Double v1, Double v2, double w1, double w2) {
        if (v1 == null && v2 == null) return null;
        if (v1 == null) return v2;
        if (v2 == null) return v1;
        return v1 * w1 + v2 * w2;
    }

    // 协议相关常量
    public static final String TARGET_PROTOCOL = "lane-sensor-sample-v4";
    public static final String[] PROTOCOLS = {
        "lane-sensor-sample-v2", "lane-sensor-sample-v3", "lane-sensor-sample-v4"
    };
} 